#include "task.h"

task_info_t *now_task;
uint16_t tmp;
uint8_t sign = 0;
void task_creat(FUN fun) {
  task_info_t *new_task = (task_info_t *)malloc(sizeof(task_info_t));
  new_task->fun_address = (uint16_t)fun;
  uint16_t *sp = (uint16_t *)malloc(sizeof(uint16_t) * 32);
  new_task->sp_address = (uint16_t)sp;
  int i;
  for (i = 0; i < 32; i++) sp[i] = 0;
  sp[30] = (uint16_t)fun;
  sp[29] = 0x08;
  if (now_task == NULL) {
    now_task = new_task;
    new_task->next_task = (void *)new_task;
  } else {
    new_task->next_task = now_task->next_task;
    now_task->next_task = new_task;
  }
  new_task->sp_address = (uint16_t)(&sp[17]);
}
void task_init() {
  tmp = now_task->sp_address;
  asm(" mov.w tmp,sp");
  asm("  pop.w r15");
  asm("  pop.w r14");
  asm("  pop.w r13");
  asm("  pop.w r12");
  asm("  pop.w r11");
  asm("  pop.w r10");
  asm("  pop.w r9");
  asm("  pop.w r8");
  asm("  pop.w r7");
  asm("  pop.w r6");
  asm("  pop.w r5");
  asm("  pop.w r4");
  asm("  pop.w r2");
  asm("  pop.w r0");
}
#pragma vector = TIMER1_A0_VECTOR
__interrupt void Timer1_A0(void) {
  // asm("  dint");
  asm("  push.w r4");
  asm("  push.w r5");
  asm("  push.w r6");
  asm("  push.w r7");
  asm("  push.w r8");
  asm("  push.w r9");
  asm("  push.w r10");
  asm("  push.w r11");
  asm("  push.w r12");
  asm("  push.w r13");
  asm("  push.w r14");
  asm(" mov.w sp,tmp");
  now_task->sp_address = tmp;
  now_task = (now_task->next_task);
  tmp = now_task->sp_address;
  asm(" mov.w tmp,sp");
  asm("  pop.w r14");
  asm("  pop.w r13");
  asm("  pop.w r12");
  asm("  pop.w r11");
  asm("  pop.w r10");
  asm("  pop.w r9");
  asm("  pop.w r8");
  asm("  pop.w r7");
  asm("  pop.w r6");
  asm("  pop.w r5");
  asm("  pop.w r4");
  switch (TA1IV) {
    default:
      break;
  }
}
